#include <iostream>
#include "bodytraced_guidance.h"
#include "px4_interface.h"
#include "guidance_filter/BlosSim.h"

int main() {
    printf("build_branch:\t%s\nbuild_sha1:\t%s\nbuild_time:\t%s %s\n",
           GIT_BRANCH_NAME, GIT_COMMIT_SHA1, __DATE__, __TIME__);

    // 获取飞机状态
    Px4Interface px4_interface(26540, 24580, "127.0.0.1");
    UavStatus px4_info;

    // 获取模拟的视线角信息
    ZmqClient  zmq_client("172.25.176.1", 14552, 40);
    TargetInfo target_info;
    LineOfSight blos, blos_rate;

    // 制导控制器
    ControllerInterface*  controller_ptr  = new BodyTracedGuidance;

    mavlink_set_attitude_target_t att_cmd;

    uint64_t timer_us = currentTimeUs();
    std::thread request_msg(&ZmqClient::request_msg, &zmq_client, "Target/1" , false);
    while(true) {
        // 判断是否获得飞控信息
        if (px4_interface.updated(px4_info) && px4_info.home_pos.time_usec > 0) 
        {

            // 从zmq协议获取目标位置
            zmq_client.updated(target_info);

            // 若目标位置更新足够快
            uint64_t time_diff = elapsedTimeMs(target_info.time_stamp_ms);

            // 根据相对位置关系，模拟视线角
            pos_to_BLOS(px4_info, target_info, blos);

            // 计算飞行姿态信息，roll， pitch， yaw
            matrix::Eulerf euler_rad = matrix::Eulerf (matrix::Quatf(px4_info.att_q.q1,px4_info.att_q.q2,px4_info.att_q.q3,px4_info.att_q.q4));
            controller_ptr->set_att_status(euler_rad(0), euler_rad(1), euler_rad(2));

            // 给控制器视线角信息输入
            controller_ptr->set_LOS_status(blos.vertical, blos.horizontal,
                                            blos_rate.vertical, blos_rate.horizontal);
            controller_ptr->set_airspeed(px4_info.vfr_hud.groundspeed);

            // 计算制导控制量
            controller_ptr->calc_att_thr_cmd(att_cmd);

            // 制导控制量发给飞控
            px4_interface.send_att_cmd_to_px4(att_cmd);

            timer_us = currentTimeUs();

            //算到目标的实时横纵距离
            mavlink_local_position_ned_t px4_local_pos;
            float N_target, E_target, D_target;
            globallocalconverter_tolocal(target_info.lat, target_info.lon, target_info.alt,
                                            &N_target, &E_target, &D_target);
            globallocalconverter_tolocal(px4_info.globalPos.lat * 1e-7,
                                            px4_info.globalPos.lon * 1e-7,
                                            px4_info.globalPos.alt * 1e-3f,
                                            &px4_local_pos.x, &px4_local_pos.y, &px4_local_pos.z);
            float deta_x = abs(px4_local_pos.x - N_target);
            float deta_y = abs(px4_local_pos.y - E_target);
            float deta_z = abs(px4_local_pos.z) - abs(D_target)-1;
            float h_dis = sqrt(pow(deta_x,2) + pow(deta_y,2));
            std::cout << "飞机自身高度： " << -(px4_local_pos.z) << ' ';
            std::cout << "水平方向距离： "
                        << h_dis << " "
                        << "垂直方向距离： "
                        << deta_z << " "
                        << std::endl;


        } else {
            // 没有获取到飞控信息，等待
            usleep(50 * 1000);
        }

    }
    request_msg.join();
    return 0;
}
